/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include "servo.h"

#define DBG_SECTION_NAME    "servo"
#define DBG_LEVEL           DBG_LOG

/**
 * @brief create a servo object
 * @param pwm pwm device name
 * @param channel pwm channel
 * @param angle servo angle range
 * @param pluse_min the minimum of pwm pluse width that servo can receive   (unit:ns
 * @param pluse_max the maximum of pwm pluse width that servo can receive   (unit:ns
 * @return the pointer of structure servo if success
 */
servo_t servo_create(const char * pwm, int channel, float angle, unsigned int pluse_min, unsigned int pluse_max)
{

    rvm_pwm_drv_register(0);

    // Malloc memory for new servo
    servo_t new_servo = (servo_t) malloc(sizeof(struct servo));
    if(new_servo == NULL)
    {
        aos_cli_printf("Falied to allocate memory for new servo");
        return NULL;
    }

    new_servo->pwmdev = (rvm_dev_t *)rvm_hal_pwm_open(pwm); /* 查找PWM设备 */
    if (new_servo->pwmdev == NULL)
    {
        aos_cli_printf("Falied to find device on %s", pwm);
        servo_destroy(new_servo);
        return NULL;
    }
    new_servo->channel = channel;
    new_servo->angle_maximum = angle; 
    if (pluse_max == 0 || pluse_min == 0)
    {
        new_servo->pluse_maximum = SERVO_DEFAULT_PULSE_MAX;
        new_servo->pluse_minimum = SERVO_DEFAULT_PULSE_MIN;
    }
    else
    {
        new_servo->pluse_maximum = pluse_max;
        new_servo->pluse_minimum = pluse_min;
    }

    return new_servo;
}

signed long servo_destroy(servo_t servo)
{
    CHECK_RET_WITH_RET(servo, 0);

    aos_cli_printf("Free servo");
    free(servo);

    return 0;
}

signed long servo_enable(servo_t servo)
{
    CHECK_RET_WITH_RET(servo, 0);

    aos_cli_printf("Enabling servo");
    rvm_hal_pwm_start(servo->pwmdev, servo->channel);

    return 0;
}

signed long servo_disable(servo_t servo)
{
    CHECK_RET_WITH_RET(servo, 0);

    aos_cli_printf("Disabling servo");

    rvm_hal_pwm_stop(servo->pwmdev, servo->channel);

    return 0;
}

signed long servo_reset(servo_t servo)
{
    return 0;
}

signed long servo_set_angle(servo_t servo, float angle)
{
    CHECK_RET_WITH_RET(servo, 0);
    rvm_hal_pwm_config_t config;
    float percentage =0;
    
    float set_point = servo->pluse_minimum + ((servo->pluse_maximum - servo->pluse_minimum) * (angle / servo->angle_maximum));
    percentage = set_point * SERVO_PERIOD/ 1000;
    config.duty_cycle = percentage;
    config.freq       = SERVO_PERIOD;
    config.polarity   = RVM_HAL_PWM_POLARITY_INVERSED;
    rvm_hal_pwm_config(servo->pwmdev, &config, servo->channel);
    
    return 0;
}
